#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <unistd.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <time.h>
#include <sys/time.h>
#include "i2c-dev.h"

/* PMBUS Commands */
#define CMD_PAGE				0x00
#define CMD_VOUT_MODE			0x20
#define CMD_READ_VOUT			0x8B
#define CMD_READ_IOUT			0x8C
#define CMD_READ_POUT			0x96
#define CMD_POWER_GOOD_OFF		0x5F
#define CMD_POWER_GOOD_ON		0x5E
#define CMD_VOUT_UV_FAULT_LIMIT	0x44
#define CMD_VOUT_UV_WARN_LIMIT	0x43
#define CMD_VOUT_MARGIN_LOW		0x26
#define CMD_VOUT_COMMAND		0x21
#define CMD_VOUT_MARGIN_HIGH	0x25
#define CMD_VOUT_OV_WARN_LIMIT	0x42
#define CMD_VOUT_OV_FAULT_LIMIT	0x40
#define CMD_VOUT_MAX			0x24

struct voltage_rail {
	char *name;
	unsigned char device;
	unsigned char page;
	double average_current;
	double average_power;
};

struct voltage_rail zcu104_rails[] = {
	{
		name			: "VCCINT   ",
		device			: 0x43,
		page			: 0x00,
		average_current	: 0.0,
		average_power	: 0.0
	},
	{
		name			: "VCC1V8   ",
		device			: 0x43,
		page			: 0x01,
		average_current	: 0.0,
		average_power	: 0.0
	}
};

double linear11ToFloat(unsigned char highByte, unsigned char lowByte) {
	unsigned short combinedWord;
	signed char exponent;
	signed short mantissa;
	double current;

	combinedWord = highByte;
	combinedWord <<= 8;
	combinedWord += lowByte;

	exponent = combinedWord >> 11;
	mantissa = combinedWord & 0x7ff;

	/* Sign extend the exponent and the mantissa */
	if(exponent > 0x0f) {
		exponent |= 0xe0;
	}
	if(mantissa > 0x03ff) {
		mantissa |= 0xf800;
	}

	current = mantissa * pow(2.0, exponent);

	return (float)current;
}

float readVoltage(int iic_fd, unsigned char deviceAddress, unsigned char pageAddress) {
	float voltage;
	int status;
	char VOUT_MODE;
	if (ioctl(iic_fd, I2C_SLAVE_FORCE, deviceAddress) < 0) {
		printf("ERROR: Unable to set I2C slave address 0x%02X\n", deviceAddress);
		exit(1);
	}

	status = i2c_smbus_write_byte_data(iic_fd, CMD_PAGE, pageAddress);
	if (status < 0) {
		printf("ERROR: Unable to write page address to I2C slave at 0x%02X: %d\n", deviceAddress, status);
		exit(1);
	}

	VOUT_MODE = i2c_smbus_read_byte_data(iic_fd, CMD_VOUT_MODE);
	if (status < 0) {
		printf("ERROR: Unable to read VOUT_MODE address to I2C slave at 0x%02X: %d\n", deviceAddress, status);
		exit(1);
	}

	/* Read in the voltage value */
	status = i2c_smbus_read_word_data(iic_fd, CMD_READ_VOUT);
	if(status < 0) {
		printf("ERROR: Unable to read VOUT on I2C slave at 0x%02X: %d\n", deviceAddress, status);
		exit(1);
	}

	voltage = status*pow(2.0, VOUT_MODE-32);
	return voltage;
}

float readCurrent(int iic_fd, unsigned char deviceAddress, unsigned char pageAddress) {
	double current;
	int status;

	if (ioctl(iic_fd, I2C_SLAVE_FORCE, deviceAddress) < 0) {
		printf("ERROR: Unable to set I2C slave address 0x%02X\n", deviceAddress);
		exit(1);
	}

	status = i2c_smbus_write_byte_data(iic_fd, CMD_PAGE, pageAddress);
	if (status < 0) {
		printf("ERROR: Unable to write page address to I2C slave at 0x%02X: %d\n", deviceAddress, status);
		exit(1);
	}

	status = i2c_smbus_read_word_data(iic_fd, CMD_READ_IOUT);
	if(status < 0) {
		printf("ERROR: Unable to read IOUT on I2C slave at 0x%02X: %d\n", deviceAddress, status);
		exit(1);
	}

	current = linear11ToFloat((unsigned char)((status >> 8) & 0xff), (unsigned char)(status & 0xff));
	return current;
}

float scaleVoltage(int iic_fd, unsigned char device_address, unsigned char page, float desired_voltage) {

	int status;
	float sample_voltage;
	int k = 0;
	char VOUT_MODE;
	__u16 VOUT_MAX,VOUT_OV_FAULT_LIMIT,VOUT_OV_WARN_LIMIT,VOUT_MARGIN_HIGH,VOUT_COMMAND,VOUT_MARGIN_LOW,VOUT_UV_WARN_LIMIT,VOUT_UV_FAULT_LIMIT,POWER_GOOD_ON,POWER_GOOD_OFF;

	float voltage_min=0.30;
	float voltage_max=1.00;

	sample_voltage = readVoltage(iic_fd, device_address, page);
	VOUT_MODE = i2c_smbus_read_byte_data(iic_fd, CMD_VOUT_MODE);

	if (desired_voltage <= voltage_max && desired_voltage >= voltage_min) {

		VOUT_COMMAND = (__u16)(desired_voltage/pow(2.0, VOUT_MODE-32));

		VOUT_MAX =(__u16)(VOUT_COMMAND*1.6);
		VOUT_OV_FAULT_LIMIT =(__u16)(VOUT_COMMAND*1.1);
		VOUT_OV_WARN_LIMIT=(__u16)(VOUT_COMMAND*1.075);
		VOUT_MARGIN_HIGH=(__u16)(VOUT_COMMAND*1.05);
		VOUT_MARGIN_LOW=(__u16)(VOUT_COMMAND*0.95);
		VOUT_UV_WARN_LIMIT=(__u16)(VOUT_COMMAND*0.925);
		VOUT_UV_FAULT_LIMIT=(__u16)(VOUT_COMMAND*0.9);
		POWER_GOOD_ON=(__u16)(VOUT_COMMAND*0.925);
		POWER_GOOD_OFF=(__u16)(VOUT_COMMAND*0.9);

		if (ioctl(iic_fd, I2C_SLAVE_FORCE, device_address) < 0) {
			printf("scale_voltage: ERROR 07: Unable to set I2C slave address 0x%02X\n", device_address);
			exit(1);
		}

		status = i2c_smbus_write_byte_data(iic_fd, CMD_PAGE, page);
		if (status < 0) {
			printf("scale_voltage: ERROR 08: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
			exit(1);
		}
/*
		printf("POWER_GOOD_OFF: 0x%02X: %d\n", CMD_POWER_GOOD_OFF, POWER_GOOD_OFF);
		printf("POWER_GOOD_ON: 0x%02X: %d\n", CMD_POWER_GOOD_ON, POWER_GOOD_ON);
		printf("VOUT_UV_FAULT_LIMIT: 0x%02X: %d\n", CMD_VOUT_UV_FAULT_LIMIT, VOUT_UV_FAULT_LIMIT);
		printf("VOUT_UV_WARN_LIMIT: 0x%02X: %d\n", CMD_VOUT_UV_WARN_LIMIT, VOUT_UV_WARN_LIMIT);
		printf("VOUT_MARGIN_LOW: 0x%02X: %d\n", CMD_VOUT_MARGIN_LOW, VOUT_MARGIN_LOW);
		printf("VOUT_MARGIN_HIGH: 0x%02X: %d\n", CMD_VOUT_MARGIN_HIGH, VOUT_MARGIN_HIGH);
		printf("VOUT_OV_WARN_LIMIT: 0x%02X: %d\n", CMD_VOUT_OV_WARN_LIMIT, VOUT_OV_WARN_LIMIT);
		printf("VOUT_OV_FAULT_LIMIT: 0x%02X: %d\n", CMD_VOUT_OV_FAULT_LIMIT, VOUT_OV_FAULT_LIMIT);
		printf("VOUT_MAX: 0x%02X: %d\n", CMD_VOUT_MAX, VOUT_MAX);
		printf("VOUT_COMMAND: 0x%02X: %d\n", CMD_VOUT_COMMAND, VOUT_COMMAND);
*/

		if (sample_voltage > desired_voltage)//scaling voltage down
		{

			status = i2c_smbus_write_word_data(iic_fd,CMD_POWER_GOOD_OFF,POWER_GOOD_OFF);
			if (status < 0) {
				printf("ERROR 09: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_POWER_GOOD_ON,POWER_GOOD_ON);
			if (status < 0) {
				printf("ERROR 13: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_UV_FAULT_LIMIT,VOUT_UV_FAULT_LIMIT);
			if (status < 0) {
				printf("ERROR 11: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_UV_WARN_LIMIT,VOUT_UV_WARN_LIMIT);
			if (status < 0) {
				printf("ERROR 12: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_MARGIN_LOW,VOUT_MARGIN_LOW);
			if (status < 0) {
				printf("ERROR 14: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_COMMAND,VOUT_COMMAND);
			if (status < 0) {
				printf("ERROR 15: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_MARGIN_HIGH,VOUT_MARGIN_HIGH);
			if (status < 0) {
				printf("ERROR 16: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_OV_WARN_LIMIT,VOUT_OV_WARN_LIMIT);
			if (status < 0) {
				printf("ERROR 17: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_OV_FAULT_LIMIT,VOUT_OV_FAULT_LIMIT);
			if (status < 0) {
				printf("ERROR 18: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_MAX,VOUT_MAX);
			if (status < 0) {
				printf("ERROR 19: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

		} 
		else //scaling voltage up
		{

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_OV_FAULT_LIMIT,VOUT_OV_FAULT_LIMIT);
			if (status < 0) {
				printf("ERROR 21: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_OV_WARN_LIMIT,VOUT_OV_WARN_LIMIT);
			if (status < 0) {
				printf("ERROR 22: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_MARGIN_HIGH,VOUT_MARGIN_HIGH);
			if (status < 0) {
				printf("ERROR 23: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_COMMAND,VOUT_COMMAND);
			if (status < 0) {
				printf("ERROR 24: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_MARGIN_LOW,VOUT_MARGIN_LOW);
			if (status < 0) {
				printf("ERROR 25: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_UV_WARN_LIMIT,VOUT_UV_WARN_LIMIT);
			if (status < 0) {
				printf("ERROR 27: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_VOUT_UV_FAULT_LIMIT,VOUT_UV_FAULT_LIMIT);
			if (status < 0) {
				printf("ERROR 29: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_POWER_GOOD_ON,POWER_GOOD_ON);
			if (status < 0) {
				printf("ERROR 26: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}

			status = i2c_smbus_write_word_data(iic_fd,CMD_POWER_GOOD_OFF,POWER_GOOD_OFF);
			if (status < 0) {
				printf("ERROR 30: Unable to write page address to I2C slave at 0x%02X: %d\n", device_address, status);
				exit(1);
			}
		}
	}
}

int main(int argc, char* argv[])
{
	float Voltage = (float)atoi(argv[1])/1000.0;
	int iic_fd = open("/dev/i2c-4", O_RDWR);
	if (iic_fd < 0) {
		printf("ERROR: Unable to open /dev/i2c-4 for PMBus access: %d\n", iic_fd);
		exit(1);
	}
	timeval start,end;
	gettimeofday(&start, NULL);
	scaleVoltage(iic_fd, zcu104_rails[0].device, zcu104_rails[0].page, Voltage);
	gettimeofday(&end, NULL);
	printf("VCCINT is set to %fV, costs %luus\n",readVoltage(iic_fd, zcu104_rails[0].device, zcu104_rails[0].page),(end.tv_sec-start.tv_sec)*1000000+end.tv_usec-start.tv_usec);
}
